#!/usr/bin/python3
import struct
import sys
import math
import hisdheader
from myerror import *

DEGTORAD=math.pi/180.0
RADTODEG=180.0/math.pi
SCLUNIT=1.525878906250000e-05
ERROR_END=100

class HisdPixLin2LonLat:
    def __init__(self,hisd):
        self.hisd=hisd
    def pixlin_to_lonlat(self,pix,lin,sd,sn,s1,s2,s3,sxy):
        coff=struct.unpack("f",self.hisd.proj_info["coff"])[0]
        loff=struct.unpack("f",self.hisd.proj_info["loff"])[0]
        cfac=int.from_bytes(self.hisd.proj_info["cfac"],sys.byteorder)
        lfac=int.from_bytes(self.hisd.proj_info["lfac"],sys.byteorder)
        satDis=struct.unpack("d",self.hisd.proj_info["satDis"])[0]
        projParam3=struct.unpack("d",self.hisd.proj_info["projParam3"])[0]
        subLon=struct.unpack("d",self.hisd.proj_info["subLon"])[0]
        projParamSd=struct.unpack("d",self.hisd.proj_info["projParamSd"])[0]

        # (0) init
        self.lon=-9999
        self.lat=-9999
        # (1) pix,lin ==> c,l
        c=pix
        l=lin
        # (2) the intermediate coordinates (x,y)
	    # Global Specification 4.4.4 Scaling Function 
	    #    c = COFF + nint(x * 2^-16 * CFAC)
	    #    l = LOFF + nint(y * 2^-16 * LFAC)
	    # The intermediate coordinates (x,y) are as follows :
	    #    x = (c -COFF) / (2^-16 * CFAC)
	    #    y = (l -LOFF) / (2^-16 * LFAC)
	    #    SCLUNIT = 2^-16
        x=DEGTORAD*(c-coff)/(SCLUNIT*cfac)
        y=DEGTORAD*(l-loff)/(SCLUNIT*lfac)
        # (3) longtitude,latitude
		# Global Specification 4.4.3.2
		# The invers projection function is as follows : 
		#   lon = arctan(S2/S1) + sub_lon
		#   lat = arctan( (Req^2/Rpol^2) * S3 / Sxy )
		# 
		# Thererin the variables S1,S2,S3,Sxy are as follows :
		#    S1  = Rs - Sn * cos(x) * cos(y)
		#    S2  = Sn * sin(x) * cos(y)
		#    S3  =-Sn * sin(y)
		#    Sxy = sqrt(S1^2 + S2^2)
		#    Sn  =(Rs * cos(x) * cos(y) - Sd ) /
		#         (cos(y) * cos(y) + (Req^2/Rpol^2) * sin(y) * sin(y))
		#    Sd  =sqrt( (Rs * cos(x) * cos(y))^2
		#               - ( cos(y) * cos(y) + (Req^2/Rpol^2) * sin(y) * sin(y) )
		#               * (Rs^2 - Req^2)
		# The variables Rs,Rpol,Req,(Req^2/Rpol^2),(Rs^2 - Req^2) are as follows :
		#    Rs  : distance from Earth center to satellite= head->proj->satDis
		#    Rpol: polar radius of the Earth              = head->proj->polrRadius
		#    Req : equator raidus of the Earth            = head->proj->eqtrRadius
		#    (Req^2/Rpol^2)                               = head->proj->projParam3
		#    (Rs^2 - Req^2)                               = head->proj->projParamSd
        sd = (satDis*math.cos(x)*math.cos(y))*(satDis*math.cos(x)*math.cos(y))-(math.cos(y) * math.cos(y)+projParam3*math.sin(y)*math.sin(y))*projParamSd
        if sd<0:
            return ERROR_END
        else:
            sd=math.sqrt(sd)
        sn=(satDis+math.cos(x)*math.cos(y)-sd)/(math.cos(y)*math.cos(y)+projParam3*math.sin(y)*math.sin(y))
        s1=satDis-sn*math.cos(x)*math.cos(y)
        s2=sn*math.sin(x)*math.cos(y)
        s3=-sn*math.sin(y)
        sxy=math.sqrt(s1*s1+s2*s2)
        self.lon=RADTODEG*math.atan2(s2,s1)+subLon
        self.lat=RADTODEG*math.atan(projParam3*s3/sxy)

        # (4) check longtitude
        while self.lon > 180.0:
            self.lon = self.hisd.lon-360.0
        while self.lon <-180.0:
            self.lon =self.hisd.lon+360.0
        

    def lonlat_to_pixlin(self,lon:float,lat:float):
        projParam2=struct.unpack("d",self.hisd.proj_info["projParam2"])[0]
        projParam1=struct.unpack("d",self.hisd.proj_info["projParam1"])[0]
        polrRadius=struct.unpack("d",self.hisd.proj_info["polrRadius"])[0]
        satDis=struct.unpack("d",self.hisd.proj_info["satDis"])[0]
        subLon=struct.unpack("d",self.hisd.proj_info["subLon"])[0]
        coff=struct.unpack("f",self.hisd.proj_info["coff"])[0]
        loff=struct.unpack("f",self.hisd.proj_info["loff"])[0]
        cfac=struct.unpack("I",self.hisd.proj_info["cfac"])[0]
        lfac=struct.unpack("I",self.hisd.proj_info["lfac"])[0]
        # (1) init
        self.pix=-9999.0
        self.lin=-9999.0
        # (2) check latitude
        if lat<-90.0 or 90.0<lat:
            return ERROR_END
        # (3) check longitude
        while lon > 180.0:
            lon = lon-360.0
        while lon <-180.0:
            lon =lon+360.0
        # (4) degree to radian
        lon = lon * DEGTORAD; # [rad]
        lat = lat * DEGTORAD; # [rad]
        # (5) geocentric latitude
	    # Global Specification 4.4.3.2
	    # phi = arctan( (Rpol^2)/(Req^2) * tan(lat) )
	    # 
	    # (Rpol^2)/(Req^2) = head->proj->projParam2
        phi = math.atan( projParam2 * math.tan(lat) )
        # (6) The length of Re
	    # Re = (Rpol) / sqrt( 1 - (Req^2 - Rpol^2) / Req^2 * cos^2(phi) )
	    #
	    # Rpol = head->proj->polrRadius
	    # (Req^2 - Rpol^2) / Req^2 = head->proj->projParam1
        Re = polrRadius / math.sqrt(1 - projParam1 * math.cos(phi) * math.cos(phi))
        # (7) The cartesian components of the vector rs result as follows:
	    # r1 = h - Re * cos(phi) * cos(Le-Ld)
	    # r2 =    -Re * cos(phi) * sin(Le-Ld)
	    # r3 =     Re * sin(phi)
	    #
	    # Le : longitude
	    # Ld : sub_lon = head->proj->subLon
	    # h  : distance from Earth's center to satellite (=head->proj->satDis)
        r1=satDis-Re*math.cos(phi)*math.cos( lon - subLon * DEGTORAD )
        r2=-Re*math.cos(phi)*math.sin(lon-subLon*DEGTORAD)
        r3=Re*math.sin(phi)
        # (8) check seeablibity
        if 0<r1*(r1-satDis)+r2*r2+r3*r3:
            return ERROR_END
        # (9) The projection function is as follows:
	    # x  = arctan(-r2/r1)
	    # y  = arcsin(r3/rn)
	    # rn = sqrt(r1^2 + r2^2 + r3^2)
        rn = math.sqrt(r1*r1 + r2*r2 + r3*r3)
        x = math.atan2(-r2,r1) * RADTODEG
        y = math.asin(-r3/rn) * RADTODEG
        # (10)
	    # Global Specification 4.4.4
	    # c  = COFF + nint(x * 2^-16 * CFAC)
	    # l  = LOFF + nint(y * 2^-16 * LFAC)
        c=coff + x * SCLUNIT * cfac
        l=loff + y * SCLUNIT * lfac
        self.pix=c
        self.lin=l
        return NORMAL_END
